//package com.hitqz.robot.dispatch.netty.service.impl;
//
//import cn.hutool.core.util.StrUtil;
//import cn.hutool.extra.spring.SpringUtil;
//import com.baomidou.mybatisplus.core.conditions.query.LambdaQueryWrapper;
//import com.baomidou.mybatisplus.core.toolkit.Wrappers;
//import com.google.common.cache.Cache;
//import com.hitqz.robot.api.business.dto.RobotClientSummaryDto;
//import com.hitqz.robot.api.business.entity.DispatchResourceEntity;
//import com.hitqz.robot.api.business.entity.DispatchResourceRecordEntity;
//import com.hitqz.robot.api.business.entity.RobotEntity;
//import com.hitqz.robot.api.business.entity.RobotGroupEntity;
//import com.hitqz.robot.api.dispatch.protocol.dto.RobotApplyPathDto;
//import com.hitqz.robot.api.dispatch.protocol.dto.RobotCmdNavDto;
//import com.hitqz.robot.api.dispatch.protocol.dto.RobotMapPosDto;
//import com.hitqz.robot.biz.service.DispatchRecordService;
//import com.hitqz.robot.biz.service.DispatchResourceRecordService;
//import com.hitqz.robot.biz.service.DispatchResourceService;
//import com.hitqz.robot.biz.service.RobotMapPosService;
//import com.hitqz.robot.dispatch.netty.strategy.AlgorithmStrategy;
//import com.hitqz.robot.dispatch.netty.util.CheZhouAgvUtil;
//import com.hitqz.robot.dispatch.netty.util.JtsUtil;
//import lombok.RequiredArgsConstructor;
//import lombok.extern.slf4j.Slf4j;
//import org.locationtech.jts.geom.Polygon;
//import org.springframework.stereotype.Service;
//
//import java.time.LocalDateTime;
//import java.util.*;
//import java.util.concurrent.*;
//import java.util.concurrent.locks.ReentrantLock;
//import java.util.stream.Collectors;
//
///**
// * @author xupkun
// * @date 2025/4/14
// */
//@Service("cheZhouAgvStrategy")
//@RequiredArgsConstructor
//@Slf4j
//public class CheZhouAgvService implements AlgorithmStrategy {
//
//    private final DispatchResourceService dispatchResourceService;
//
//    private final DispatchResourceRecordService dispatchResourceRecordService;
//
//    private final DispatchRecordService dispatchRecordService;
//
//    private final RobotMapPosService robotMapPosService;
//
//    /**
//     *
//     */
//    private final Object lockC = new Object();
//
//    /**
//     *
//     */
//    private final Object lockB = new Object();
//
//    /**
//     *
//     */
//    private final Object lockA = new Object();
//    private final Object lockD = new Object();
//    private final Object lockF = new Object();
//
//
//    private final Map<String, ReentrantLock> lockMap = new ConcurrentHashMap<>();
//
//    private ReentrantLock getLock(String robotSn) {
//        return lockMap.computeIfAbsent(robotSn, k -> new ReentrantLock());
//    }
//
//    private static ThreadPoolExecutor threadPoolExecutor = new ThreadPoolExecutor(
//            200,
//            200,
//            60,
//            TimeUnit.SECONDS,
//            new LinkedBlockingQueue<Runnable>(200),
//            Executors.defaultThreadFactory(),
//            new ThreadPoolExecutor.DiscardOldestPolicy());
//
//
//    @Override
//    public void changePath(RobotGroupEntity robotGroup, RobotApplyPathDto pathDto) {
//        if (pathDto.getPathList().isEmpty()) {
//            CheZhouAgvUtil.clearRobot(pathDto.getClientId());
//        }
//        //pathDto.getPathList().remove(0);
//        // 去掉自身
//        log.info("提交路径前往：{}---{}", pathDto.getClientId(), pathDto.getPathList().stream().map(RobotMapPosDto::getCode).
//                collect(Collectors.joining("-")));
//        CheZhouAgvUtil.sendPathFlag.put(pathDto.getClientId(), false);
//        CheZhouAgvUtil.sendStopFlag.put(pathDto.getClientId(), false);
//        CheZhouAgvUtil.submitPath(pathDto);
//    }
//
//
//    @Override
//    public void calculate(int groupId, List<RobotEntity> robotEntityList) {
//        Cache<String, RobotClientSummaryDto> robotCache = SpringUtil.getBean("robotCache");
//        LambdaQueryWrapper<DispatchResourceEntity> wrapper = Wrappers.lambdaQuery();
//        wrapper.eq(DispatchResourceEntity::getMapCode, robotEntityList.get(0).getMapCode());
//        if (robotEntityList.isEmpty()) {
//            return;
//        }
//        try {
//            for (RobotEntity robotEntity : robotEntityList) {
//                RobotApplyPathDto robotApplyPathDto = CheZhouAgvUtil.robotApplyPathDtoMap.get(robotEntity.getRobotSn());
//                RobotClientSummaryDto robotClientSummaryDto = robotCache.getIfPresent(robotEntity.getRobotSn());
//                if (robotClientSummaryDto == null) {
//                    continue;
//                }
//                if (robotClientSummaryDto.getLogicOnlineStatus() == 0) {
//                    continue;
//                }
//                String nearStation = robotClientSummaryDto.getPosition().getLaserPos().getNearPosCode();
//                String lastArrivePos = robotClientSummaryDto.getPosition().getLaserPos().getLastPosCode();
//                if (StrUtil.isBlank(nearStation)) {
//                    nearStation = lastArrivePos;
//                }
//                LinkedList<RobotMapPosDto> currentPathList = Optional.ofNullable(robotApplyPathDto).orElse(new RobotApplyPathDto()).getPathList();
//                int pathListSize = 0;
//                if (currentPathList != null && !currentPathList.isEmpty()) {
//                    pathListSize = currentPathList.size();
//                }
//                int arrivePosIndex = -1;
//                try {
//                    // 如果路径列表较大，可以考虑使用哈希表加速查找
//                    if (currentPathList != null && !currentPathList.isEmpty()) {
//                        Map<String, Integer> pathCodeIndexMap = new HashMap<>();
//                        for (int i = 0; i < pathListSize; i++) {
//                            String code = currentPathList.get(i).getCode();
//                            if (code != null) {
//                                pathCodeIndexMap.put(code.toUpperCase(), i); // 转换为大写以支持忽略大小写的比较
//                            }
//                        }
//
//                        // 快速查找匹配的路径点位
//                        if (pathCodeIndexMap.containsKey(nearStation.toUpperCase())) {
//                            arrivePosIndex = pathCodeIndexMap.get(nearStation.toUpperCase());
//                        }
//                    }
//
//                } catch (NullPointerException e) {
//                    log.error("解析当前位置时发生空指针异常: {}", e.getMessage(), e);
//                    // 可以根据业务需求决定是否抛出异常或返回默认值
//                } catch (Exception e) {
//                    log.error("解析当前位置时发生未知异常: {}", e.getMessage(), e);
//                    // 根据业务需求处理其他类型的异常
//                }
//                if (arrivePosIndex > 0) {
//                    log.info("说明可能网络信号不太好，删除前面的元素");
//                    // 使用 Iterator 安全地删除元素
//                    Iterator<RobotMapPosDto> iterator = currentPathList.iterator();
//                    int count = 0;
//                    while (iterator.hasNext()) {
//                        if (count < arrivePosIndex) {
//                            iterator.remove(); // 安全删除
//                        } else {
//                            break; // 提前退出循环，避免不必要的迭代
//                        }
//                        count++;
//                    }
//                    arrivePosIndex = 0;
//                }
//                // 计算当前点到下一个路径的距离，然后提前进行申请
//                RobotMapPosDto robotMapPosDto = null;
//                if (robotApplyPathDto != null && robotApplyPathDto.getPathList() != null) {
//                    LinkedList<RobotMapPosDto> pathList = robotApplyPathDto.getPathList();
//                    robotMapPosDto = pathList.get(0);
//                    if (nearStation.equals(robotMapPosDto.getCode())) {
//                        pathList.remove(0);
//                        if (!pathList.isEmpty()) {
//                            robotMapPosDto = pathList.get(0);
//                        }
//                    }
//                }
//                boolean applyNext = applyNext(robotEntity, nearStation, robotMapPosDto);
//                processPath(robotEntity, nearStation, arrivePosIndex, currentPathList);
//            }
//        } catch (Exception e) {
//            log.error("获取资源信息失败", e);
//        }
//
//    }
//
//    /**
//     * 对路径数据进行处理
//     */
//    public void processPath(RobotEntity robotEntity, String nearStation, int arrivePosIndex, LinkedList<RobotMapPosDto> currentPathList) {
//        lockC(lockC, () -> {
//            if (arrivePosIndex == 0 && !currentPathList.isEmpty()) {
//                logArrivalInfo(nearStation, currentPathList, arrivePosIndex, robotEntity);
//                updateDispatchResourceRecords(robotEntity);
//            }
//        });
//    }
//
//    private void lockC(Object lock, Runnable task) {
//        synchronized (lock) {
//            try {
//                task.run();
//            } catch (Exception e) {
//                log.error("路径处理过程中发生异常: {}", e.getMessage(), e);
//            }
//        }
//    }
//
//    private void logArrivalInfo(String nearStation, LinkedList<RobotMapPosDto> currentPathList, int arrivePosIndex, RobotEntity robotEntity) {
//        String pathCodes = currentPathList.stream()
//                .map(RobotMapPosDto::getCode)
//                .collect(Collectors.joining("-"));
//        //log.info("到达点位是 {} 计算之前的路径: {}", nearStation, pathCodes);
//        RobotApplyPathDto robotApplyPathDto = CheZhouAgvUtil.robotApplyPathDtoMap.get(robotEntity.getRobotSn());
//        //log.info("AGV执行剩余路径111111:" + currentPathList.stream()
//        //        .map(RobotMapPosDto::getCode)
//        //        .collect(Collectors.joining("-")));
//        RobotMapPosDto robotMapPosDto = currentPathList.get(arrivePosIndex);
//        if (nearStation.equals(robotMapPosDto.getCode())) {
//            currentPathList.remove(arrivePosIndex);
//            robotApplyPathDto.setPathList(currentPathList);
//            CheZhouAgvUtil.robotApplyPathDtoMap.put(robotEntity.getRobotSn(), robotApplyPathDto);
//            String remainingPathCodes = robotApplyPathDto.getPathList().stream()
//                    .map(RobotMapPosDto::getCode)
//                    .collect(Collectors.joining("-"));
//            //log.info("AGV执行剩余路径22222: {}", remainingPathCodes);
//            if (currentPathList.isEmpty()) {
//                CheZhouAgvUtil.clearRobot(robotEntity.getRobotSn());
//            }
//        }
//
//    }
//
//    private void updateDispatchResourceRecords(RobotEntity robotEntity) {
//        List<DispatchResourceRecordEntity> dispatchResourceRecords = dispatchResourceRecordService.list(
//                Wrappers.<DispatchResourceRecordEntity>lambdaQuery()
//                        .eq(DispatchResourceRecordEntity::getOccupyRobot, robotEntity.getRobotSn())
//                        .eq(DispatchResourceRecordEntity::getStatus, 1)
//        );
//        dispatchResourceRecords.forEach(record -> {
//            record.setEndTime(LocalDateTime.now());
//            record.setStatus(3);
//            dispatchResourceRecordService.updateById(record);
//        });
//    }
//
//
//    private void send2RobotNav(RobotEntity robotEntity, RobotMapPosDto dto) {
//        // 参数校验
//        try {
//            // 检查是否需要发送路径
//            if (!CheZhouAgvUtil.sendPathFlag.computeIfAbsent(robotEntity.getRobotSn(), key -> false)) {
//                CheZhouAgvUtil.sendPathFlag.put(robotEntity.getRobotSn(), true); // 标记为已发送
//                log.info("机器人 {} 下发可以前往位置 {}", robotEntity.getRobotSn(), dto.getCode());
//                // 写入区域被占用
//                // 调用外部方法发送最终位置
//                CheZhouAgvUtil.goFinalPos(robotEntity.getRobotSn(), dto);
//            }
//        } catch (Exception e) {
//            log.error("发送路径失败，机器人编号: {}, 目标位置: {}", robotEntity.getRobotSn(), dto.getCode(), e);
//        }
//    }
//
//
//    /**
//     * 修改导航状态
//     *
//     * @param status 导航状态
//     */
//    public static void changeNavStatus(RobotEntity robotEntity, int status, RobotMapPosDto robotMapPosDto) {
//        // 确保 clientId 不为空
//        synchronized (robotEntity.getRobotSn().intern()) {
//            // 获取 sendPathFlag 的值并避免多次调用 get 方法
//            Boolean sendPathFlagValue = CheZhouAgvUtil.sendPathFlag.get(robotEntity.getRobotSn());
//            if (sendPathFlagValue != null && sendPathFlagValue) {
//                try {
//                    Cache<String, RobotClientSummaryDto> robotCache = SpringUtil.getBean("robotCache");
//                    if (robotCache != null) {
//                        RobotClientSummaryDto robotClientSummaryDto = robotCache.getIfPresent(robotEntity.getRobotSn());
//                        assert robotClientSummaryDto != null;
//                        int taskStatus = robotClientSummaryDto.getStatus().getNavStatus();
//                        if (taskStatus == 2 && status == 1) {
//                            status = 3;
//                            // 取消导航
//                            SpringUtil.getApplicationContext().publishEvent(new RobotCmdNavDto(robotEntity.getRobotSn(), status));
//                            TimeUnit.SECONDS.sleep(1);
//                        }
//                        if (taskStatus == 6 && status == 2) {
//                            CheZhouAgvUtil.goFinalPos(robotEntity.getRobotSn(), robotMapPosDto);
//                            TimeUnit.SECONDS.sleep(1);
//                        }
//                    }
//                    if (status == 1) {
//                        CheZhouAgvUtil.sendStopFlag.put(robotEntity.getRobotSn(), true);
//
//                    }
//                    if (status == 2) {
//                        CheZhouAgvUtil.sendStopFlag.put(robotEntity.getRobotSn(), false);
//
//                    }
//                } catch (Exception e) {
//                    // 捕获异常并记录详细日志
//                    log.error("发布导航状态事件失败，clientId: {}, status: {}", robotEntity.getRobotSn(), status, e);
//                }
//            }
//        }
//    }
//
//
//    public boolean applyNext(RobotEntity robotEntity, String nearStation, RobotMapPosDto robotMapPosDto) {
//        synchronized (robotEntity.getRobotSn().intern()) {
//            Cache<String, Polygon> robotPosPolygon = SpringUtil.getBean("robotPosPolygon");
//            Polygon polygon = robotPosPolygon.getIfPresent(robotEntity.getRobotSn());
//            if (polygon == null) {
//                return false;
//            }
//            // 如果下一个点是null 表示当前是不动的
//            String pointCode;
//            if (robotMapPosDto != null) {
//                pointCode = robotMapPosDto.getCode();
//
//            } else {
//                pointCode = nearStation;
//            }
//            synchronized (lockB) {
//                if (robotMapPosDto == null) {
//                    // 没有下一个目标点
//                    // 当前车是不动的写入当前被占用
//                    DispatchResourceRecordEntity dispatchResourceRecordEntity = dispatchResourceRecordService.getOne(
//                            Wrappers.<DispatchResourceRecordEntity>lambdaQuery()
//                                    .eq(DispatchResourceRecordEntity::getAreaId, 0)
//                                    .eq(DispatchResourceRecordEntity::getOccupyRobot, robotEntity.getRobotSn())
//                                    .eq(DispatchResourceRecordEntity::getRemark, pointCode)
//                                    .eq(DispatchResourceRecordEntity::getStatus, 1)
//                                    .last("limit 1")
//                    );
//                    if (dispatchResourceRecordEntity == null) {
//                        List<DispatchResourceEntity> list = findPosInArea(robotEntity, nearStation);
//                        if (list.isEmpty()) {
//                            dispatchResourceRecordEntity = new DispatchResourceRecordEntity();
//                            dispatchResourceRecordEntity.setRemark(pointCode);
//                            dispatchResourceRecordEntity.setOccupyRobot(robotEntity.getRobotSn());
//                            dispatchResourceRecordEntity.setStatus(1);
//                            // 查询当前点是否在区域内
//                            dispatchResourceRecordEntity.setAreaId(0);
//                            dispatchResourceRecordEntity.setEndTime(LocalDateTime.now());
//                            dispatchResourceRecordEntity.setStartTime(LocalDateTime.now());
//                            dispatchResourceRecordService.save(dispatchResourceRecordEntity);
//                        } else {
//                            list.forEach(t -> {
//                                synchronized (lockD) {
//                                    DispatchResourceRecordEntity entity =
//                                            dispatchResourceRecordService.getOne(
//                                                    Wrappers.<DispatchResourceRecordEntity>lambdaQuery()
//                                                            .eq(DispatchResourceRecordEntity::getAreaId, t.getId())
//                                                            .eq(DispatchResourceRecordEntity::getOccupyRobot, robotEntity.getRobotSn())
//                                                            .eq(DispatchResourceRecordEntity::getRemark, pointCode)
//                                                            .eq(DispatchResourceRecordEntity::getStatus, 1)
//                                                            .last("limit 1")
//                                            );
//                                    if (entity == null) {
//                                        entity = new DispatchResourceRecordEntity();
//                                        entity.setRemark(pointCode);
//                                        entity.setOccupyRobot(robotEntity.getRobotSn());
//                                        entity.setStatus(1);
//                                        // 查询当前点是否在区域内
//                                        entity.setAreaId(t.getId());
//                                        entity.setEndTime(LocalDateTime.now());
//                                        entity.setStartTime(LocalDateTime.now());
//                                        dispatchResourceRecordService.save(entity);
//                                    }
//
//                                }
//
//                            });
//                        }
//                    }
//                } else {
//                    if (CheZhouAgvUtil.sendPathFlag.get(robotEntity.getRobotSn()) != null && !CheZhouAgvUtil.sendPathFlag.get(robotEntity.getRobotSn())) {
//                        // 没有下发过导航
//                        boolean applyArea = applyArea(robotEntity, pointCode);
//                        if (applyArea) {
//                            RobotApplyPathDto robotApplyPathDto = CheZhouAgvUtil.robotApplyPathDtoMap.get(robotEntity.getRobotSn());
//                            if (robotApplyPathDto != null) {
//                                RobotMapPosDto finalPos = robotApplyPathDto.getPathList().get(robotApplyPathDto.getPathList().size() - 1);
//                                send2RobotNav(robotEntity, finalPos);
//                            }
//                        }
//                    } else {
//                        // 下发过
//                        double distance = JtsUtil.getDistance(polygon, JtsUtil.getPoint(robotMapPosDto.getPosX(), robotMapPosDto.getPosY()));
//                        if (distance <= 3.0) {
//                            boolean applyResult = applyArea(robotEntity, pointCode);
//                            if (!applyResult) {
//                                changeNavStatus(robotEntity, 1, robotMapPosDto);
//                            } else {
//                                //log.info("申请下一个点位通过的结果: {}-{}", pointCode, applyResult);
//                                changeNavStatus(robotEntity, 2, robotMapPosDto);
//                            }
//                        }
//                    }
//
//                }
//            }
//        }
//        return true;
//    }
//
//
//    /**
//     * 申请区域
//     *
//     * @param robotEntity
//     * @param nextPointCode
//     * @return
//     */
//    private boolean applyArea(RobotEntity robotEntity, String nextPointCode) {
//        ReentrantLock lock = getLock(robotEntity.getRobotSn());
//        lock.lock();
//        boolean writeNext = true;
//        try {
//            // 查询出来下一个点所在的区域
//            List<DispatchResourceEntity> prepareArea = findPosInArea(robotEntity, nextPointCode);
//            if (prepareArea.isEmpty()) {
//                DispatchResourceRecordEntity dispatchResourceRecordEntity = dispatchResourceRecordService.getOne(
//                        Wrappers.<DispatchResourceRecordEntity>lambdaQuery()
//                                .eq(DispatchResourceRecordEntity::getAreaId, 0)
//                                .eq(DispatchResourceRecordEntity::getOccupyRobot, robotEntity.getRobotSn())
//                                .eq(DispatchResourceRecordEntity::getRemark, nextPointCode)
//                                .eq(DispatchResourceRecordEntity::getStatus, 1)
//                                .last("limit 1")
//                );
//                if (dispatchResourceRecordEntity == null) {
//                    dispatchResourceRecordEntity = new DispatchResourceRecordEntity();
//                    dispatchResourceRecordEntity.setRemark(nextPointCode);
//                    dispatchResourceRecordEntity.setOccupyRobot(robotEntity.getRobotSn());
//                    dispatchResourceRecordEntity.setStatus(1);
//                    // 查询当前点是否在区域内
//                    dispatchResourceRecordEntity.setAreaId(0);
//                    dispatchResourceRecordEntity.setEndTime(LocalDateTime.now());
//                    dispatchResourceRecordEntity.setStartTime(LocalDateTime.now());
//                    dispatchResourceRecordService.save(dispatchResourceRecordEntity);
//                }
//                dispatchResourceRecordService.update(
//                        Wrappers.<DispatchResourceRecordEntity>lambdaUpdate()
//                                .set(DispatchResourceRecordEntity::getStatus, 3)
//                                .ne(DispatchResourceRecordEntity::getRemark, nextPointCode)
//                                .eq(DispatchResourceRecordEntity::getOccupyRobot, robotEntity.getRobotSn())
//                                .eq(DispatchResourceRecordEntity::getStatus, 1)
//                );
//            } else {
//                // 查询准备申请的区域是否已经被占用
//                List<DispatchResourceRecordEntity> existingArea = dispatchResourceRecordService.list(
//                        Wrappers.<DispatchResourceRecordEntity>lambdaQuery()
//                                .ne(DispatchResourceRecordEntity::getOccupyRobot, robotEntity.getRobotSn())
//                                .eq(DispatchResourceRecordEntity::getStatus, 1)
//                                .in(DispatchResourceRecordEntity::getAreaId,
//                                        prepareArea.stream().map(DispatchResourceEntity::getId).toList())
//                );
//                if (existingArea != null && !existingArea.isEmpty()) {
//                    return false;
//                }
//                // 查询是否存在当前的
//                // 写入准备申请的区域
//                prepareArea.forEach(t -> {
//                    DispatchResourceRecordEntity entity = dispatchResourceRecordService.getOne(
//                            Wrappers.<DispatchResourceRecordEntity>lambdaQuery()
//                                    .eq(DispatchResourceRecordEntity::getAreaId,
//                                            t.getId())
//                                    .eq(DispatchResourceRecordEntity::getOccupyRobot, robotEntity.getRobotSn())
//                                    .eq(DispatchResourceRecordEntity::getStatus, 1));
//
//                    if (entity == null) {
//                        entity = new DispatchResourceRecordEntity();
//                        entity.setRemark(nextPointCode);
//                        entity.setOccupyRobot(robotEntity.getRobotSn());
//                        entity.setStatus(1);
//                        // 查询当前点是否在区域内
//                        entity.setAreaId(t.getId());
//                        entity.setEndTime(LocalDateTime.now());
//                        entity.setStartTime(LocalDateTime.now());
//                        dispatchResourceRecordService.save(entity);
//                    }
//                });
//
//                dispatchResourceRecordService.update(
//                        Wrappers.<DispatchResourceRecordEntity>lambdaUpdate()
//                                .set(DispatchResourceRecordEntity::getStatus, 3)
//                                .notIn(DispatchResourceRecordEntity::getAreaId,
//                                        prepareArea.stream().map(DispatchResourceEntity::getId).toList())
//                                .eq(DispatchResourceRecordEntity::getOccupyRobot, robotEntity.getRobotSn())
//                                .eq(DispatchResourceRecordEntity::getStatus, 1)
//                );
//            }
//        } catch (Exception e) {
//
//        } finally {
//            lock.unlock();
//        }
//        return writeNext;
//    }
//
//
//    /**
//     * 查询申请的点位是不是在区域内
//     *
//     * @param robotEntity
//     * @param posCode
//     * @return
//     */
//    private List<DispatchResourceEntity> findPosInArea(RobotEntity robotEntity, String posCode) {
//        List<DispatchResourceEntity> allResource = dispatchResourceService.list(
//                Wrappers.<DispatchResourceEntity>lambdaQuery()
//                        .eq(DispatchResourceEntity::getMapCode, robotEntity.getMapCode())
//        );
//        return allResource.stream().filter(item -> {
//            String[] path = item.getPathList().split(",");
//            for (String s : path) {
//                if (s.equals(posCode)) {
//                    return true;
//                }
//            }
//            return false;
//        }).toList();
//    }
//
//
//}
